/****************************************************************************
 * Xunyi Inc..
 * This program runs on companion computer (XU4), and control pixhawk 
 * autopilot via MAVLink protocol.
 ****************************************************************************/

/**

 *@author Plum Lee (liyang@xyitech.com)
 
 */

// ------------------------------------------------------------------------------
//   Includes
// ------------------------------------------------------------------------------

#include "main.h"

// ------------------------------------------------------------------------------
//   TOP
// ------------------------------------------------------------------------------
int
top (int argc, char **argv)
{

	// ---------------------------------------------------------------------------
	//   INSTANTIATION   
	// ---------------------------------------------------------------------------
	
	// pilot
	char *uart_name = (char*)"/dev/ttySAC0";
	int baudrate = 921600;	
	serial_port_c serial_port(uart_name, baudrate);
	autopilot_interface_c pilot(&serial_port);
	wireless_c wireless_serial;
	
	serial_port_quit         = &serial_port;
	autopilot_interface_quit = &pilot;
	wireless_serial_quit     = &wireless_serial;
	signal(SIGINT,quit_handler);
	signal(SIGPIPE, SIG_IGN);
	printf("top initialization!\n");
	
	//vision(camera)
	xy_vision_c xy_vision;

	//ultrasonic
	ultrasonic_c xy_ultra;
	waypoint_list_c waypoints_flight;
	xy_network_c xy_network(pilot, waypoints_flight);
	xy_health_c xy_health(xy_network);
	// ---------------------------------------------------------------------------
	//   INITIALIZE 
	// ---------------------------------------------------------------------------

	//start
	xy_network.start();
	serial_port.start();
	wireless_serial.start();
	if(xy_ultra.setup(NULL, 9600) < 0)
	{
		xy_ultra.ultra_bad_flag = ULTRA_BAD_BIT;
	}
	xy_vision.setup_capture();
	pilot.start_read();

	xy_health.health_status |= xy_ultra.ultra_bad_flag | xy_vision.cap_unopen_flag | wireless_serial.wireless_bad_flag;
	xy_health.report_health_status(xy_health.health_status);
	// --------------------------------------------------------------------------
	//   RUN FLIGHT
	// --------------------------------------------------------------------------

	fly(pilot, xy_network, xy_vision, xy_ultra, waypoints_flight, xy_health, wireless_serial);

	// --------------------------------------------------------------------------
	//   THREAD and PORT SHUTDOWN
	// --------------------------------------------------------------------------
	
	//pilot.disable_offboard_control();
	printf("over 1\n");
	if (pilot.read_tid)
	{
		pilot.stop_read();
	}
	printf("over 2\n");
	if (pilot.write_tid)
	{
		pilot.stop_write();
	}	
	serial_port.stop();
	// --------------------------------------------------------------------------
	//   DONE
	// --------------------------------------------------------------------------
	printf("over\n");
	return 0;

}


// ------------------------------------------------------------------------------
//   COMMANDS
// ------------------------------------------------------------------------------

void
fly(autopilot_interface_c &api, xy_network_c &xy_network, xy_vision_c &xy_vision, ultrasonic_c &xy_ultra, 
                    waypoint_list_c &waypoints_flight, xy_health_c &xy_health, wireless_c& wireless_serial)
{

	uint32_t count = 0;
	//float time = 0;
	uint32_t ctime = 1;
	uint32_t ptime = 0;
	uint32_t time_count = 0;	

	//initialize flight state 0: standby | 1: ready to fly | 2: takeoff | 3: forward | 4: landing |
	uint16_t flight_state = FLIGHT_STATE_STANDBY;
	
	//initialize mission	
	waypoint_list_c waypoints_takeoff, waypoints_land, waypoints_emer;
	waypoint_t takeoff_point, land_point;
	printf("fly initialization!\n");
	//uint16_t mission_sign = 			
	uint16_t item_target = 1;
	//safe landing
	waypoint_t cur_point;
	float dis_emland[10] = {};
	int emland_count = 0;
	waypoint_t nearest_emland;
	float initial_yaw = 0.0;
	float acceptance_radius = DIS_CRITERIA_HORIZONTAL;

	float delta_initial_x = 0;
	float delta_initial_y = 0;

	//initialize commander
	ref_navigator_c ref_navigator_recorded(SAMPLING_PERIOD);
	
	ref_navigator_c ref_navigator_flight(SAMPLING_PERIOD);
	ref_navigator_flight.direction = HORIZONTAL_NAVIGATOR;

	ref_navigator_c ref_navigator_emer(SAMPLING_PERIOD);
	ref_navigator_emer.direction = HORIZONTAL_NAVIGATOR;
	
	ref_navigator_c ref_navigator_takeoff(SAMPLING_PERIOD);
	ref_navigator_takeoff.direction = VERTICAL_NAVIGATOR;
	
	ref_navigator_c ref_navigator_land(SAMPLING_PERIOD);
	ref_navigator_land.direction = VERTICAL_NAVIGATOR;	

	printf("finish navigator initialization!\n");
	// ------------------------------------------------------------------------
	//   initialize controllers
	// ------------------------------------------------------------------------
	controller_3D_posPID_c controller_recorded(SAMPLING_PERIOD);
	controller_3D_posPID_c controller_takeoff(SAMPLING_PERIOD);
	controller_3D_posPID_c controller_flight(SAMPLING_PERIOD);
	controller_3D_posPID_c controller_land(SAMPLING_PERIOD);
	controller_angPID_c    controller_yaw(SAMPLING_PERIOD);

	d3_PID_t PID_takeoff, PID_flight, PID_land;

	xy_log_c xy_log(api, xy_ultra, xy_vision, ref_navigator_recorded, controller_recorded);

	//takeoff controller
	PID_takeoff.PID_x.Kp 		= KP_X_TAKEOFF;	
	PID_takeoff.PID_x.Ki 		= KI_X_TAKEOFF;
	PID_takeoff.PID_x.Kd 		= KD_X_TAKEOFF;
	PID_takeoff.PID_x.u_limit	= UPPER_X_TAKEOFF;
	PID_takeoff.PID_x.l_limit	= LOWER_X_TAKEOFF;

	PID_takeoff.PID_y.Kp 		= KP_Y_TAKEOFF;	
	PID_takeoff.PID_y.Ki 		= KI_Y_TAKEOFF;
	PID_takeoff.PID_y.Kd 		= KD_Y_TAKEOFF;
	PID_takeoff.PID_y.u_limit	= UPPER_Y_TAKEOFF;
	PID_takeoff.PID_y.l_limit	= LOWER_Y_TAKEOFF;

	PID_takeoff.PID_z.Kp 		= KP_Z_TAKEOFF;	
	PID_takeoff.PID_z.Ki 		= KI_Z_TAKEOFF;
	PID_takeoff.PID_z.Kd 		= KD_Z_TAKEOFF;
	PID_takeoff.PID_z.u_limit	= UPPER_Z_TAKEOFF;
	PID_takeoff.PID_z.l_limit	= LOWER_Z_TAKEOFF;
	
	controller_takeoff.set(PID_takeoff);

	//flight controller
	PID_flight.PID_x.Kp 		= KP_X_FLIGHT;	
	PID_flight.PID_x.Ki 		= KI_X_FLIGHT;
	PID_flight.PID_x.Kd 		= KD_X_FLIGHT;
	PID_flight.PID_x.u_limit	= UPPER_X_FLIGHT;
	PID_flight.PID_x.l_limit	= LOWER_X_FLIGHT;

	PID_flight.PID_y.Kp 		= KP_Y_FLIGHT;	
	PID_flight.PID_y.Ki 		= KI_Y_FLIGHT;
	PID_flight.PID_y.Kd 		= KD_Y_FLIGHT;
	PID_flight.PID_y.u_limit	= UPPER_Y_FLIGHT;
	PID_flight.PID_y.l_limit	= LOWER_Y_FLIGHT;

	PID_flight.PID_z.Kp 		= KP_Z_FLIGHT;	
	PID_flight.PID_z.Ki 		= KI_Z_FLIGHT;
	PID_flight.PID_z.Kd 		= KD_Z_FLIGHT;
	PID_flight.PID_z.u_limit	= UPPER_Z_FLIGHT;
	PID_flight.PID_z.l_limit	= LOWER_Z_FLIGHT;
	
	controller_flight.set(PID_flight);

	//land controller
	PID_land.PID_x.Kp 		= KP_X_LAND;	
	PID_land.PID_x.Ki 		= KI_X_LAND;
	PID_land.PID_x.Kd 		= KD_X_LAND;
	PID_land.PID_x.u_limit	= UPPER_X_LAND;
	PID_land.PID_x.l_limit	= LOWER_X_LAND;

	PID_land.PID_y.Kp 		= KP_Y_LAND;	
	PID_land.PID_y.Ki 		= KI_Y_LAND;
	PID_land.PID_y.Kd 		= KD_Y_LAND;
	PID_land.PID_y.u_limit	= UPPER_Y_LAND;
	PID_land.PID_y.l_limit	= LOWER_Y_LAND;

	PID_land.PID_z.Kp 		= KP_Z_LAND;	
	PID_land.PID_z.Ki 		= KI_Z_LAND;
	PID_land.PID_z.Kd 		= KD_Z_LAND;
	PID_land.PID_z.u_limit	= UPPER_Z_LAND;
	PID_land.PID_z.l_limit	= LOWER_Z_LAND;
	
	controller_land.set(PID_land);
	
	controller_yaw.set(KP_YAW, KI_YAW, KD_YAW, UPPER_YAW, LOWER_YAW);

	PID_kit_t yaw_pid;

	integrator_time_c integrator_takeoff(SAMPLING_PERIOD);
	integrator_time_c integrator_land(SAMPLING_PERIOD);
	times_counter_c flight_remote_mode_counter, flight_auto_mode_counter;
	int authen_xu4 = 1;
	//printf("0 api.current_messages.rc_channel.chan6_raw = %u\n", api.current_messages.rc_channel.chan6_raw);
	while(1) 
	{
		xy_health.health_status |= xy_ultra.ultra_bad_flag | xy_vision.cap_unopen_flag | wireless_serial.wireless_bad_flag;
		xy_health.report_health_status(xy_health.health_status);
		wireless_serial.write_message(api.relay_message);
		
		//time = SAMPLING_PERIOD * count;		
		//printf("time = %.2f\n", time);
		//printf("local_position_ned.time = %u\n", api.current_messages.local_position_ned.time_boot_ms);
		//printf("chan5_raw = %u\n", api.current_messages.rc_channel.chan5_raw);
		ctime = api.current_messages.local_position_ned.time_boot_ms;
		
		if (ctime == ptime)
		{
			time_count++;
			if (time_count > 100)
			{
				api.disable_offboard_control();
				usleep(100);
				printf("over 4\n");
				if (api.writing_status)
				{
					api.stop_write();
					usleep(100);
				}
				flight_state = FLIGHT_STATE_MANUAL;
			}		
		}
		else
		{
			time_count = 0;
		}
		ptime = ctime;				

		if (flight_remote_mode_counter.click((api.current_messages.rc_channel.chan5_raw < 1300), 5))
		{
			authen_xu4 = 0;
			printf("flight_remote_mode_counter.count = %u\n", flight_remote_mode_counter.count);
		}

		if (flight_auto_mode_counter.click((api.current_messages.rc_channel.chan5_raw > 1400), 5))
		{
			authen_xu4 = 1;
			printf("flight_auto_mode_counter.count = %u\n", flight_auto_mode_counter.count);
		}

		printf("authen_xu4 = %d\n", authen_xu4);
		
		if ((authen_xu4 == 0) && (flight_state != FLIGHT_STATE_STANDBY))
		{
			//printf("chan5_raw = %d\n", api.current_messages.rc_channel.chan5_raw);
			api.disable_offboard_control();
			usleep(5000);
			//printf("api.writing_status = %u\n", api.writing_status);
			if (api.writing_status)
			{
				api.stop_write();
				usleep(100);
			}
			flight_state = FLIGHT_STATE_MANUAL;
			//printf("over 5\n");
		}		
		//printf("2 api.current_messages.rc_channel.chan6_raw = %u\n", api.current_messages.rc_channel.chan6_raw);
		//emer_land
		//emergency land use google earth coordinate
		if(xy_network.mission_command.get_emer_land_status() == 1 && flight_state != FLIGHT_STATE_EMER && flight_state != FLIGHT_STATE_LAND)
		{	
			waypoint_t emer_all_node[7] = {};
			cur_point.x = api.current_messages.position_target_local_ned.x;
			cur_point.y = api.current_messages.position_target_local_ned.y;
			cur_point.z = (float)api.current_messages.global_position_int.relative_alt / 1000;
			xy_network.mission_command.set_command_ack(COMMAND_ACK, 2, 0);
			dis_emland[0] = sqrt(pow(cur_point.x, 2) + pow(cur_point.y, 2));
			emer_all_node[0].x = 0;
			emer_all_node[0].y = 0;
			emer_all_node[0].z = 25;
			emer_all_node[0].speed= 5;
			emer_all_node[1].x = land_point.x;
			emer_all_node[1].y = land_point.y;
			emer_all_node[1].z = land_point.z;
			emer_all_node[1].speed= 5;
			dis_emland[1] = sqrt(pow((land_point.x - cur_point.x), 2) + pow((land_point.y - cur_point.y), 2));
			printf("dis_emland[0]:%lf, dis_emland[1]--%lf\n", dis_emland[0], dis_emland[1]);

			for (emland_count = 0; emland_count < 5; emland_count++ )
			{
				emer_all_node[emland_count+2].x = waypoints_flight.emer_land_node[emland_count].x;
				emer_all_node[emland_count+2].y = waypoints_flight.emer_land_node[emland_count].y;
				emer_all_node[emland_count+2].z = waypoints_flight.emer_land_node[emland_count].z;
				emer_all_node[emland_count+2].speed = waypoints_flight.emer_land_node[emland_count].speed;
				dis_emland[2 + emland_count] = sqrt(pow((waypoints_flight.emer_land_node[emland_count].x - cur_point.x), 2) + pow((waypoints_flight.emer_land_node[emland_count].y - cur_point.y), 2));
				printf("dis_emland[%d]:%lf\n", 2 + emland_count, dis_emland[2 + emland_count]);
			}

			//find the smallest one ,set the target_emland value with the serial number
			for(int i = 0; i < 7-1; i++)
			{
				for(int j = 0; j < 7-i-1; j++)
				{
					if(dis_emland[j] > dis_emland[j+1])
					{
						float tmp_dis = dis_emland[j];
						waypoint_t tmp_node = emer_all_node[j];
						dis_emland[j] = dis_emland[j+1];
						emer_all_node[j] = emer_all_node[j+1];
						dis_emland[j+1] = tmp_dis;
						emer_all_node[j+1] = tmp_node;
					}
				}
			}

			printf("disstance is [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f] \n", dis_emland[0], dis_emland[1], dis_emland[2], dis_emland[3], dis_emland[4], dis_emland[5], dis_emland[6]);
			
			//the smallest is the 0
			nearest_emland.x = emer_all_node[0].x;
			nearest_emland.y = emer_all_node[0].y;
			nearest_emland.z = 25;
			nearest_emland.speed = 5;
			printf("nearest_emland:x:%lf,y:%lf,z:%lf\n",nearest_emland.x, nearest_emland.y, nearest_emland.z);
			waypoints_emer.insert_waypoint(1, cur_point);
			printf("cur_point:x:%lf,y:%lf,z:%lf\n",cur_point.x, cur_point.y, cur_point.z);
			waypoints_emer.insert_waypoint(2, nearest_emland);
			flight_state = FLIGHT_STATE_SET_INITIAL_YAW;		
		}

		printf("\nflight_state = %d\n", flight_state);
		printf("xy_vision: *%.4f, %.4f*\n", -xy_vision.vision_data.position.y, xy_vision.vision_data.position.x);
		printf("channel_5 = %u\n", api.current_messages.rc_channel.chan5_raw);
		//printf("api.reading_status = %u, api.writing_status = %u\n", api.reading_status, api.writing_status);

		//printf("1 Kp = %.4f, Ki = %.4f, Kd = %.4f\n", PID_land.PID_z.Kp, PID_land.PID_z.Ki, PID_land.PID_z.Kd);
		// for tuning pid parameters online
		//printf("1 api.time_to_exit_read = %d\n", api.time_to_exit_read);
		//printf("2 Kp = %.4f, Ki = %.4f, Kd = %.4f\n", PID_land.PID_z.Kp, PID_land.PID_z.Ki, PID_land.PID_z.Kd);
		//printf("api.current_messages.extended_state.landed_state = %u\n", api.current_messages.extended_state.landed_state);
		//printf("api.current_messages.heartbeat.system_status = %u\n", api.current_messages.heartbeat.system_status);
		switch (flight_state)
		{
			case FLIGHT_STATE_MANUAL:
				//printf("MANUAL\n");
				//printf("chan5_raw %d\n",api.current_messages.rc_channel.chan5_raw );
				
				if (xy_network.mission_command.cur_heartbeat.base_mode != SYS_STAT_ON_LINE)
				{
					xy_network.mission_command.set_heartbeat_mode(SYS_STAT_ON_LINE);
					waypoints_flight.clear();
					xy_network.mission_command.route_flag = 0;
				}
				xy_network.mission_command.set_heartbeat_mode(SYS_STAT_REMOTE_CONTROL);
				if (api.current_messages.heartbeat.system_status == 3)
				{
					flight_state = FLIGHT_STATE_STANDBY;
				}
				break;

			case FLIGHT_STATE_STANDBY:
				//printf("\nFLIGHT_STATE_STANDBY\n");
				if ((xy_network.mission_command.cur_heartbeat.base_mode != SYS_STAT_ON_LINE) && (xy_network.mission_command.cur_heartbeat.base_mode != SYS_STAT_ONLOAD))
				{
					xy_network.mission_command.set_heartbeat_mode(SYS_STAT_ON_LINE);
				}
				//printf("standby!!!!\n");
				//printf("status = %d!!!!\n", xy_network.mission_command.get_heartbeat_status());
				if (xy_network.mission_command.get_heartbeat_status() == SYS_STAT_ONLOAD)
				{
					printf("onload!!!!\n");
					xy_network.mission_command.set_command_ack(COMMAND_ACK, XYI_MISSION_ACCEPTED, XYI_RESULT_ACCEPTED);
					if((api.current_messages.heartbeat.system_status == 4) && (authen_xu4 == 1))
					{
						xy_log.XY_SD_Log_Setup();
						xy_log.XY_Start_Store_Log();
						flight_state = FLIGHT_STATE_READY;
						xy_network.mission_command.set_heartbeat_mode(SYS_STAT_WAIT_FLY);
					}
				}
				set_pid_online(controller_flight, controller_yaw, PID_flight, yaw_pid, acceptance_radius, xy_network);

				if (xy_network.mission_command.get_route_flag())
				{
					init_mission(waypoints_flight, xy_network, api);
					takeoff_point 	= waypoints_flight.get_headpoint();
					land_point 		= waypoints_flight.get_waypoint(waypoints_flight.count);
					
					xy_network.mission_command.set_route_flag(0);
					xy_network.mission_command.set_heartbeat_mode(SYS_STAT_ONLOAD);
					printf("waypoints_flight.count = %u\n", waypoints_flight.count);
					for(int i=1; i<=waypoints_flight.count; i++)
					{
						printf("waypoints_flight: x=%.4f, y=%.4f, z=%.4f, speed=%.4f\n", waypoints_flight.get_waypoint(i).x, waypoints_flight.get_waypoint(i).y, waypoints_flight.get_waypoint(i).z, waypoints_flight.get_waypoint(i).speed);
					}				
				}				
				break;
				
			case FLIGHT_STATE_READY:
				if(xy_network.mission_command.get_takeoff_flag() == 1)
				{
					if (api.writing_status == false)
					{
						//printf("writing!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
						api.start_write();
						usleep(100);
					}
					//printf("2 api.reading_status = %u, api.writing_status = %u\n", api.reading_status, api.writing_status);
					api.enable_offboard_control();
					usleep(100);
					//printf("3 api.reading_status = %u, api.writing_status = %u\n", api.reading_status, api.writing_status);
					flight_state = FLIGHT_STATE_TAKEOFF;
					takeoff_point.x = api.current_messages.local_position_ned.x;
					takeoff_point.y = api.current_messages.local_position_ned.y;
					xy_network.mission_command.reset_takeoff_flag();
				}
				//printf("4 api.reading_status = %u, api.writing_status = %u\n", api.reading_status, api.writing_status);
				break;
				
			case FLIGHT_STATE_TAKEOFF:
				ref_navigator_recorded = ref_navigator_takeoff;
				controller_recorded = controller_takeoff;
				xy_vision.start_capture();
				//printf("\nFLIGHT_STATE_TAKEOFF\n");					
				//printf("takeoff_point_z = %.4f, ",takeoff_point.z);			

				if (flight_takeoff(api, takeoff_point, ref_navigator_takeoff, controller_takeoff, controller_yaw, xy_vision, xy_ultra, integrator_takeoff))				
				{
					waypoint_t point_initial;
					point_initial.x = api.current_messages.local_position_ned.x;
					point_initial.y = api.current_messages.local_position_ned.y;
					point_initial.z = takeoff_point.z;
					//float initial_distance = dis_cal(api.current_messages.local_position_ned, takeoff_point);
					
					if (waypoints_flight.count != 0)
					{
						waypoints_flight.delete_waypoint(1);
					}						
					waypoints_flight.insert_waypoint(1, point_initial);					
					
					flight_state = FLIGHT_STATE_SET_INITIAL_YAW;
					xy_vision.stop_capture();
				}
				if (api.current_messages.extended_state.landed_state == 2)
				{
					xy_network.mission_command.set_heartbeat_mode(SYS_STAT_GO_UP);
				}
				break;

			case FLIGHT_STATE_SET_INITIAL_YAW:				
				//printf("delta_initial_x = %.4f, delta_initial_y = %.4f\n", delta_initial_x, delta_initial_y);
				//printf("initial_yaw = %.4f\n", initial_yaw);
				//printf("api.current_messages.attitude.yaw = %.4f\n", api.current_messages.attitude.yaw);
				//printf("yaw error = %.4f, c = %.4f\n", fabs(api.current_messages.attitude.yaw - initial_yaw), ANGLE_CRITERIA_INTITIAL);
				if (xy_network.mission_command.get_emer_land_status() == 1)
				{
					delta_initial_x = waypoints_emer.get_waypoint(2).x - waypoints_emer.get_waypoint(1).x;
					delta_initial_y = waypoints_emer.get_waypoint(2).y - waypoints_emer.get_waypoint(1).y;
					initial_yaw	= atan2(delta_initial_y, delta_initial_x);
					if (fabs(api.current_messages.attitude.yaw - initial_yaw) <= ANGLE_CRITERIA_INTITIAL)
					{
						flight_state = FLIGHT_STATE_EMER;
					}
					else
					{
						controller_flight.d3_ctrl.velocity.vx = 0;
						controller_flight.d3_ctrl.velocity.vy = 0;
						controller_flight.d3_ctrl.velocity.vz = 0;
						controller_yaw.ref = initial_yaw;
						controller_yaw.feedback = api.current_messages.attitude.yaw;
						controller_yaw.calc();
						controller_flight.d3_ctrl.yaw_ref = controller_yaw.u_ctrl;
						mavlink_set_position_target_local_ned_t sp = {0};
						set_ref_yaw_rate(controller_flight.d3_ctrl, sp);
						api.update_setpoint(sp);
					}
				}
				else
				{
					delta_initial_x = waypoints_flight.get_waypoint(2).x - waypoints_flight.get_waypoint(1).x;
					delta_initial_y = waypoints_flight.get_waypoint(2).y - waypoints_flight.get_waypoint(1).y;
					initial_yaw	= atan2(delta_initial_y, delta_initial_x);
					if (fabs(api.current_messages.attitude.yaw - initial_yaw) <= ANGLE_CRITERIA_INTITIAL)
					{
						flight_state = FLIGHT_STATE_FLIGHT;
					}
					else
					{
						controller_takeoff.d3_ctrl.velocity.vx = 0;
						controller_takeoff.d3_ctrl.velocity.vy = 0;
						controller_takeoff.d3_ctrl.velocity.vz = 0;
						controller_yaw.ref = initial_yaw;
						controller_yaw.feedback = api.current_messages.attitude.yaw;
						controller_yaw.calc();
						controller_takeoff.d3_ctrl.yaw_ref = controller_yaw.u_ctrl;
						mavlink_set_position_target_local_ned_t sp = {0};
						set_ref_yaw_rate(controller_takeoff.d3_ctrl, sp);
						api.update_setpoint(sp);
					}					
				}					
				
				printf("controller_land.output: x = %.4f, y = %.4f, z = %.4f, yaw = %.4f\n", controller_land.d3_ctrl.velocity.vx, controller_land.d3_ctrl.velocity.vy, controller_land.d3_ctrl.velocity.vz, controller_land.d3_ctrl.yaw_ref);
				break;
				
			case FLIGHT_STATE_FLIGHT:
				ref_navigator_recorded = ref_navigator_flight;
				controller_recorded = controller_flight;
				xy_network.mission_command.set_heartbeat_mode(SYS_STAT_P2P);
				
				printf("\nFLIGHT_STATE_FLIGHT\n");			
				item_target = flight_normal(api, waypoints_flight, ref_navigator_flight, controller_flight, controller_yaw, item_target, acceptance_radius);
				printf("item_target=%d\n",item_target);
				if (item_target > waypoints_flight.count)
				{
					flight_state = FLIGHT_STATE_LAND;
				}
				break;
				
			case FLIGHT_STATE_LAND:	
				ref_navigator_recorded = ref_navigator_land;
				controller_recorded = controller_land;
				if(xy_network.mission_command.get_emer_land_status() == 1)
				{
					xy_network.mission_command.set_heartbeat_mode(SYS_STAT_EMER_DOWN);
				}
				//xy_vision.start_capture();
				//printf("FLIGHT_STATE_LAND\n");				
				if (flight_land(api, land_point, ref_navigator_land, controller_land, controller_yaw, xy_vision, xy_ultra, integrator_land, xy_network))
				{
					//flight_state = FLIGHT_STATE_STANDBY;
					printf("landed_state = %d\n", api.current_messages.extended_state.landed_state);
					if (api.current_messages.extended_state.landed_state == 1)
					{
						if(xy_network.mission_command.get_emer_land_status() == 1)
						{
							xy_network.mission_command.set_heartbeat_mode(SYS_STAT_EMER_LAND);
							xy_network.mission_command.land.land_flag = 0;
						}
						else
						{
							xy_network.mission_command.set_heartbeat_mode(SYS_STAT_LAND);
						}
						xy_vision.stop_capture();
						xy_log.set_log_on_flag(0);
						flight_state = FLIGHT_STATE_MANUAL;
						api.island = 0;
					}
				}
				break;
				
			case FLIGHT_STATE_EMER:	
				ref_navigator_recorded = ref_navigator_emer;
				item_target = flight_normal(api, waypoints_emer, ref_navigator_emer, controller_flight, controller_yaw, 1, acceptance_radius);
				printf("item_target=%d\n",item_target);
				if (item_target == 2)
				{
					land_point = waypoints_emer.get_waypoint(2);
					flight_state = FLIGHT_STATE_LAND;
				}
				break;
			default: break;
		}

		usleep(SAMPLING_PERIOD * 1E6);
		count++;		
	}
	
	printf("\nFLIGHT_RETURN\n");
}



uint16_t init_mission(waypoint_list_c &waypoints, xy_network_c &xy_network, autopilot_interface_c &api)
{
	position _epos,_spos;
	XYZ eXYZ;
	Center_xyz exyz;
	int xyz_count = 0,route_count = 0;

	/*
	while(1)
	{
		if (api.current_messages.rc_channel.chan6_raw > 1400)
		{
			system("shutdown now");
			return -1;
		}
		if(xy_network.mission_command.get_route_flag() == 1)
			break;
		printf("chan6_raw = %d!!\n", api.current_messages.rc_channel.chan6_raw);
		usleep(100000);
	}
	*/
	
	_spos._lati 	= (((float) xy_network.mission_command.pilot.current_messages.gps_raw_int.lat) / 1E7 / 180) * PI;
	_spos._longti 	= (((float) xy_network.mission_command.pilot.current_messages.gps_raw_int.lon) / 1E7 / 180) * PI;
	_spos._alti 	= ((float) xy_network.mission_command.pilot.current_messages.global_position_int.relative_alt) / 1E3;
	_spos._height 	= _spos._alti ;

	printf("_spos._longti=%f, _spos._lati=%f, _spos._alti=%f \n", _spos._longti, _spos._lati, _spos._alti);
	
	waypoint_t mission_waypoints[255] = {};
	
	for(uint16_t i = 0; i < xy_network.mission_command.items->count; i++)
	{
		_epos._lati = ((xy_network.mission_command.items->gps[xyz_count] - GD2GE_LATI_DIFF_NANHU)/180) * PI;
		xyz_count++;
		_epos._longti = ((xy_network.mission_command.items->gps[xyz_count]- GD2GE_LONGTI_DIFF_NANHU)/180) * PI;
		xyz_count++;
		_epos._alti = xy_network.mission_command.items->gps[xyz_count];
		_epos._height= xy_network.mission_command.items->gps[xyz_count];
		xyz_count++;
		printf("_epos._longti=%f,_epos._lati=%f,z_epos._height=%f \n", _epos._longti,_epos._lati,_epos._alti);
		geo2XYZ(_epos, &eXYZ);
		XYZ2xyz(_spos, eXYZ, &exyz);
		mission_waypoints[i].x = exyz.x;
		mission_waypoints[i].y = exyz.y;
		mission_waypoints[i].z = _epos._height;
		mission_waypoints[i].speed= xy_network.mission_command.items->gps[xyz_count];
		xyz_count++;
		route_count++;
	}
	//uint16_t len = getArrayLen(mission_waypoints);
	uint16_t x = 0;
	for (uint16_t i = 1; i < route_count + 1; i++)
	{
		x = waypoints.insert_waypoint(i,mission_waypoints[i-1]);
	}	
	for (uint16_t i = 1; i < route_count + 1; i++)
	{
		printf("[%d] x=%f,y=%f,z=%f \n", i, waypoints.get_waypoint(i).x,waypoints.get_waypoint(i).y,waypoints.get_waypoint(i).z);
	}
	xy_network.mission_command.set_emer_land_node(_spos);
	xy_network.mission_command.set_mission_ack(MISSION_ACK, 1, 1, 0);
	return x;
}


int flight_takeoff(autopilot_interface_c &api, waypoint_t takeoff_point, ref_navigator_c &navigator_takeoff, controller_3D_posPID_c &controller_takeoff, controller_angPID_c &controller_yaw, xy_vision_c &xy_vision, ultrasonic_c &xy_ultra, integrator_time_c &integrator_takeoff)
{

	float height_ultra = 0, height_px = 0, height = 0;
	ref_position_vision_t vision_position = {0};
	
	height_px = (float)api.current_messages.global_position_int.relative_alt / 1000;

	//float vx = api.current_messages.local_position_ned.vx;
	//float vy = api.current_messages.local_position_ned.vy;
	//float vz = -api.current_messages.local_position_ned.vz;

	//printf("v: {%.3f, %.3f, %.3f}\n", vx, vy, vz);
	//printf("1 height_ultra = %.2f\n", height_ultra);
	//ultrasonic measures height
	
	if (xy_ultra.is_on)
	{
		xy_ultra.close();
	}
	height = height_px;
	//printf("height_px = %.2f, height_ultra = %.2f, height = %.2f\n", height_px, xy_ultra.ultra_data.data, height);
	controller_takeoff.feedback.position.z = height;
	//printf("2 height_ultra = %.2f\n", height_ultra);
	printf("height = %f\n",height);
	
	//vision measures x, y
	//printf("xy_vision.vision_data.gotten = %d\n", xy_vision.vision_data.gotten);
	int vision_state = xy_vision.get_data(&vision_position, 0x02, height);
	vision_position = correction_vision(vision_position, api, height);

	controller_takeoff.feedback.position.x = api.current_messages.local_position_ned.x;
	controller_takeoff.feedback.position.y = api.current_messages.local_position_ned.y;
				
	//printf("vision_position: x = %.4f, y = %.4f\n", vision_position.x, vision_position.y);
	if (xy_vision.vision_data.gotten == 0 )//image has not been identified
	{
		navigator_takeoff.startpoint.x = takeoff_point.x;
		navigator_takeoff.startpoint.y = takeoff_point.y;
		//navigator_takeoff.startpoint.z = height;
		navigator_takeoff.endpoint.x = takeoff_point.x;
		navigator_takeoff.endpoint.y = takeoff_point.y;
	}	
	else
	{
		if (fabs(api.current_messages.attitude.yaw) < 0.035)
		{
			if ( vision_state == 0)
			{
				//set the startpoint as current position
				navigator_takeoff.startpoint.x = navigator_takeoff.ref_point.position.x;
				navigator_takeoff.startpoint.y = navigator_takeoff.ref_point.position.y;

				navigator_takeoff.endpoint.x = controller_takeoff.feedback.position.x - vision_position.y;
				navigator_takeoff.endpoint.y = controller_takeoff.feedback.position.y + vision_position.x;
				
				navigator_takeoff.reset(1, 0);
			}
		}		

		//float xy_dis = sqrt(pow(controller_takeoff.feedback.position.x - navigator_takeoff.endpoint.x, 2) + pow(controller_takeoff.feedback.position.y - navigator_takeoff.endpoint.y, 2));

	}	

	//printf("feedback: x = %.4f, y = %.4f\n", controller_takeoff.feedback.position.x, controller_takeoff.feedback.position.y);
	navigator_takeoff.endpoint.z = takeoff_point.z;
	//printf("takeoff x = %f, y = %f, z = %f\n", navigator_takeoff.endpoint.x, navigator_takeoff.endpoint.y, vision_position.z);
	//navigator generate reference point

	if (height > HEIGHT_ACCELERATION)
	{
		navigator_takeoff.refspeed = takeoff_point.speed;
	}
	else
	{
		navigator_takeoff.refspeed = takeoff_point.speed * (fabs(height) / HEIGHT_ACCELERATION) + 0.2;
		if (navigator_takeoff.refspeed > takeoff_point.speed)
		{
			navigator_takeoff.refspeed = takeoff_point.speed;
		}
	}
	
	navigator_takeoff.d3_generate();
	controller_takeoff.ref = navigator_takeoff.ref_point;
	
	if (height > HEIGHT_TO_SET_YAW)
	{
		controller_yaw.ref = 0;
		controller_yaw.feedback = api.current_messages.attitude.yaw;
		controller_takeoff.feedback.yaw = controller_yaw.feedback;
		controller_yaw.calc();
		controller_takeoff.ref.yaw = controller_yaw.u_ctrl;
	}
	else
	{
		controller_takeoff.ref.yaw = 0;
	}
	//printf("navigator_takeoff.ref_point.yaw = %.4f\n", navigator_takeoff.ref_point.yaw);	

	//printf("controller_yaw.ref = %.4f, feedback = %.4f, u = %.4f\n", controller_yaw.ref, controller_yaw.feedback, controller_yaw.u_ctrl);

	printf("controller_takeoff.ref: x = %.4f, y = %.4f, z = %.4f, yaw = %.4f\n", controller_takeoff.ref.position.x, controller_takeoff.ref.position.y, controller_takeoff.ref.position.z, controller_takeoff.ref.yaw);
	printf("controller_takeoff.feedback: x = %.4f, y = %.4f, z = %.4f, yaw = %.4f\n", controller_takeoff.feedback.position.x, controller_takeoff.feedback.position.y, controller_takeoff.feedback.position.z, controller_takeoff.feedback.yaw);
	
	//printf("error = %.4f ",controller_climb.error);
	if (fabs(takeoff_point.z - controller_takeoff.feedback.position.z) < DIS_CRITERIA_TAKEOFF)
	{
		xy_vision.stop_capture();
		return 1;
	}
	else
	{
		mavlink_set_position_target_local_ned_t sp = {0};
		//mavlink_set_position_target_local_ned_t ip = api.initial_position;

		//int *c_windup;
		//c_windup = 
		controller_takeoff.calc();
		//printf("controller_takeoff.controller_z.error = %.3f, ", controller_takeoff.controller_z.error);
		//printf("controller_takeoff.d3_ctrl.velocity.vz = %.3f\n", controller_takeoff.d3_ctrl.velocity.vz);
		printf("controller_takeoff.output: x = %.4f, y = %.4f, z = %.4f, yaw_rate = %.4f\n", controller_takeoff.d3_ctrl.velocity.vx, controller_takeoff.d3_ctrl.velocity.vy, controller_takeoff.d3_ctrl.velocity.vz, controller_takeoff.d3_ctrl.yaw_ref);
		set_ref_yaw_rate(controller_takeoff.d3_ctrl, sp);
		api.update_setpoint(sp);
		return 0;
	}
	
}


uint16_t flight_normal(autopilot_interface_c &api, waypoint_list_c waypoints, ref_navigator_c &ref_cmd, controller_3D_posPID_c &controller_flight, controller_angPID_c &controller_yaw, int item, float acceptance_radius)
{
		
	float vx = api.current_messages.local_position_ned.vx;
	float vy = api.current_messages.local_position_ned.vy;
	float vz = -api.current_messages.local_position_ned.vz;
	printf("v: {%.3f, %.3f, %.3f}\n\n", vx, vy, vz);

	float radius = 3;
	//printf("item=%d\n",item);
	if (item > waypoints.count)
	{
		return 0;
	}
	else 
	{
		mavlink_set_position_target_local_ned_t sp = {0};
		ref_cmd.startpoint 	= waypoints.get_waypoint(item);
		printf("item = %d,x = %f,y=%f\n",item,waypoints.get_waypoint(item).x,waypoints.get_waypoint(item).y);
		ref_cmd.endpoint   	= waypoints.get_waypoint(item + 1);
		printf("item = %d,x = %f,y=%f\n",item+1,waypoints.get_waypoint(item+1).x,waypoints.get_waypoint(item+1).y);
		ref_cmd.refspeed   	= ref_cmd.endpoint.speed;
		ref_cmd.d3_generate();
		controller_flight.ref = ref_cmd.ref_point;
		controller_yaw.ref = ref_cmd.ref_point.yaw;
		controller_yaw.feedback = api.current_messages.attitude.yaw;
		controller_yaw.calc();
		controller_flight.ref.yaw = controller_yaw.u_ctrl;
		
		//printf("api.local = (%f, %f, %f)\n ", api.current_messages.local_position_ned.x, api.current_messages.local_position_ned.y, api.current_messages.local_position_ned.z);
		//get feedback
		controller_flight.feedback.position.x = api.current_messages.local_position_ned.x;
		controller_flight.feedback.position.y = api.current_messages.local_position_ned.y;
		controller_flight.feedback.position.z = (float)api.current_messages.global_position_int.relative_alt / 1000;

		if (item == waypoints.count)
		{
			radius = 5;
		}
		else
		{
			radius = acceptance_radius;
		}
		
		if (dis_cal(api.current_messages.local_position_ned, ref_cmd.endpoint) < radius)
		{
			ref_cmd.reset(1, 1);
			return item + 1;
		}
		else
		{			 
			controller_flight.calc();			
			set_ref_yaw_rate(controller_flight.d3_ctrl, sp);
			api.update_setpoint(sp);
			return item;
		}		
	}
	
}


int flight_land(autopilot_interface_c &api, waypoint_t land_point, ref_navigator_c &navigator_land, controller_3D_posPID_c &controller_land, controller_angPID_c &controller_yaw, xy_vision_c &xy_vision, ultrasonic_c &xy_ultra, integrator_time_c &integrator_land, xy_network_c &xy_network)
{
	
	float height = 0, height_px = 0, height_ultra = 0; 
	ref_position_vision_t vision_position = {0};
	
	height_px = (float)api.current_messages.global_position_int.relative_alt / 1000;

	float vx = api.current_messages.local_position_ned.vx;
	float vy = api.current_messages.local_position_ned.vy;
	float vz = -api.current_messages.local_position_ned.vz;

	printf("v: {%.3f, %.3f, %.3f}\n\n", vx, vy, vz);
	//printf("1 height_ultra = %.2f\n", height_ultra);
	//ultrasonic measures height
	if (height_px < HEIGHT_ULTRA_OPEN)
	{
		if (xy_ultra.is_on == false)
		{
			xy_ultra.open();
		}
		printf("xy_ultra.confidence_flag = %d\n", xy_ultra.confidence_flag);
		if (xy_ultra.ultra_data.gotten != 0)
		{
			int ultra_state = xy_ultra.get_filtered_data(&height_ultra, 0x02);			
			if (ultra_state == 0)
			{
				xy_ultra.valid_count++;

				if (xy_ultra.valid_count > 5)
				{
					xy_ultra.confidence_flag = 1;
				}

				if (xy_ultra.confidence_flag)
				{
					height = height_ultra;
					integrator_land.initial_value_1.z = height;
					integrator_land.reset(0, 0, 1);
				}
				else
				{
					height = height_px;
				}
			}
			else
			{
				if (xy_ultra.confidence_flag)
				{
					if (integrator_land.initial_value_1.z != 0)
					{
						integrator_land.f.z = vz;
						integrator_land.order_1();
						height = integrator_land.order_1_value.z;
						printf("height = %.4f\n", height);
					}
				}				
				else
				{
					height = height_px;
				}					
			}			
		}
		else
		{
			height = height_px;
		}
	}
	else
	{
		if (xy_ultra.is_on)
		{
			xy_ultra.close();
		}
		height = height_px;
	}
	
	printf("height_px = %.2f, height_ultra = %.2f, height = %.2f\n", height_px, xy_ultra.ultra_data.data, height);
	controller_land.feedback.position.z = height;

	controller_land.feedback.position.x = api.current_messages.local_position_ned.x;
	controller_land.feedback.position.y = api.current_messages.local_position_ned.y;
	
	if (height > HEIGHT_OPEN_CAMERA)
	{
		xy_network.mission_command.set_heartbeat_mode(SYS_STAT_GO_DOWN);
		navigator_land.startpoint.x = land_point.x;  
		navigator_land.startpoint.y = land_point.y;
		navigator_land.startpoint.z = land_point.z;
		navigator_land.endpoint.x = land_point.x;
		navigator_land.endpoint.y = land_point.y;
		navigator_land.endpoint.z = HEIGHT_HOVER_FIND_MARK;
		navigator_land.refspeed = DESCEND_RATE;
		printf("land_point: x=%.4f, y=%.4f, z=%.4f\n", land_point.x, land_point.y, land_point.z);
	}	
	else
	{
		if (height < HEIGHT_DECELERATION)
		{
			navigator_land.refspeed = (DESCEND_RATE - LANDING_RATE) * (height - HEIGHT_DIFFERENCE) / HEIGHT_DECELERATION + LANDING_RATE;
			printf("navigator_land.refspeed = %.4f\n", navigator_land.refspeed);
		}

		xy_vision.start_capture();
		
		if ((navigator_land.endpoint.z < HEIGHT_HOVER_FIND_MARK + 0.1) && (navigator_land.endpoint.z > HEIGHT_HOVER_FIND_MARK - 0.1))
		{
			printf("xy_network.mission_command.cur_heartbeat.base_mode = %u\n", xy_network.mission_command.cur_heartbeat.base_mode);
			if (xy_network.mission_command.cur_heartbeat.base_mode != SYS_STAT_MANUAL_CONTROL)
			{
				xy_network.mission_command.set_heartbeat_mode(SYS_STAT_IMAGE_IDENTIFY);
				printf("2 xy_network.mission_command.cur_heartbeat.base_mode = %u\n", xy_network.mission_command.cur_heartbeat.base_mode);
			}			
		}
		
		int vision_state = xy_vision.get_data(&vision_position, 0x02, height);
		//printf("xy_network.mission_command.control.cmd_status = %.2f\n", xy_network.mission_command.control.cmd_status);
		//printf("xy_network.mission_command.control.horizontal_control = %.2f\n", xy_network.mission_command.control.horizontal_control);
		//printf("xy_network.mission_command.control.vertical_control = %.2f\n", xy_network.mission_command.control.vertical_control);
		if (xy_network.mission_command.control.cmd_status < 0.1)
		{
			if (vision_state == 0)
			{
				printf("SYS_STAT_GOBACK_DOWN\n");
				xy_network.mission_command.set_heartbeat_mode(SYS_STAT_LAND);				
				vision_position = correction_vision(vision_position, api, height);
				navigator_land.startpoint.x = navigator_land.ref_point.position.x;
				navigator_land.startpoint.y = navigator_land.ref_point.position.y;

				navigator_land.endpoint.x = controller_land.feedback.position.x - vision_position.y;
				navigator_land.endpoint.y = controller_land.feedback.position.y + vision_position.x;
				navigator_land.reset(1, 0);
				navigator_land.startpoint.z = navigator_land.ref_point.position.z;
				navigator_land.endpoint.z = HEIGHT_DIFFERENCE;
				navigator_land.reset(0, 1);
				printf("navigator: start.z=%.4f, end.z=%.4f, ref.z=%.4f, time_v=%.4f\n", navigator_land.startpoint.z, navigator_land.endpoint.z, navigator_land.ref_point.position.z, navigator_land.mission_time_v);
			}			
		}
		else
		{
			//printf("SYS_STAT_MANUAL_CONTROL\n");
			xy_network.mission_command.set_heartbeat_mode(SYS_STAT_MANUAL_CONTROL);
			float offset_x = 0, offset_y = 0;
			if ((xy_network.mission_command.control.horizontal_control > 0) && (!xy_network.mission_command.cloud_control_running))
			{
				printf("manual_h\n");
				xy_network.mission_command.cloud_control_running = 1;
				float image_x = xy_network.mission_command.control.offset_x_per;
				float image_y = xy_network.mission_command.control.offset_y_per;
				printf("offset_x_per = %.4f, offset_y_per = %.4f\n", image_x, image_y);
				distance_transformation(image_x, image_y, offset_x, offset_y, height, xy_vision);
				navigator_land.startpoint.x = navigator_land.ref_point.position.x;
				navigator_land.startpoint.y = navigator_land.ref_point.position.y;
				navigator_land.endpoint.x = controller_land.feedback.position.x - offset_y;
				navigator_land.endpoint.y = controller_land.feedback.position.y + offset_x;
				printf("offset_x = %.4f, offset_y = %.4f\n", offset_x, offset_y);
				navigator_land.reset(1, 0);
				xy_network.mission_command.control.offset_x_per = 0;
				xy_network.mission_command.control.offset_y_per = 0;				
				xy_network.mission_command.control.horizontal_control = -1;				
			}
			else if ((xy_network.mission_command.control.vertical_control > 0) && (!xy_network.mission_command.cloud_control_running))
			{
				printf("manual_v\n");
				xy_network.mission_command.cloud_control_running = 1;
				navigator_land.startpoint.z = navigator_land.ref_point.position.z;
				navigator_land.endpoint.z = xy_network.mission_command.control.offset_z_per;
				navigator_land.reset(0, 1);
				xy_network.mission_command.control.vertical_control= -1;
				xy_network.mission_command.control.offset_z_per = 0;
				printf("offset_z = %.4f\n", xy_network.mission_command.control.offset_z_per);				
			}
			//printf("offset_x = %.4f, offset_y = %.4f\n", offset_x, offset_y);
			//printf("offset_z = %.4f\n", xy_network.mission_command.control.offset_z_per);
		}		
	}
	printf("xy_vision.vision_data.gotten = %d\n", xy_vision.vision_data.gotten);	
	
	float xy_dis = sqrt(pow(controller_land.feedback.position.x - navigator_land.endpoint.x, 2) + pow(controller_land.feedback.position.y - navigator_land.endpoint.y, 2));
	navigator_land.d3_generate();
	navigator_land.ref_point.yaw = 0;
	controller_land.ref = navigator_land.ref_point;
	controller_yaw.ref = navigator_land.ref_point.yaw;
	controller_yaw.feedback = api.current_messages.attitude.yaw;
	controller_yaw.calc();
	controller_land.ref.yaw = controller_yaw.u_ctrl;
	printf("navigator_land.ref_point: x = %.4f, y = %.4f, z = %.4f, z_refspeed = %.4f\n", navigator_land.ref_point.position.x, navigator_land.ref_point.position.y, navigator_land.ref_point.position.z, navigator_land.ref_point.position.speed);
	printf("controller_land.feedback: x = %.4f, y = %.4f, z = %.4f\n", controller_land.feedback.position.x, controller_land.feedback.position.y, controller_land.feedback.position.z);

	mavlink_set_position_target_local_ned_t sp = {0};
	float height_to_landing = 2;
	
	if (xy_ultra.confidence_flag)
	{
		height_to_landing = HEIGHT_LANDING;
	}
	else
	{
		height_to_landing = HEIGHT_DIFFERENCE + 3;
	}
	
	printf("height_to_landing = %.4f\n", height_to_landing);
	printf("landing_height = %.4f\n", height);
	
	if (height < height_to_landing)
	{
		api.disable_offboard_control();
		//usleep(SAMPLING_PERIOD * 1E6);
		usleep(500);
		api.island = 1;
		//usleep(SAMPLING_PERIOD * 1E6 / 2);
		usleep(70000);
		api.land();
		usleep(100);
		return 1;
	}			
	else	
	{
		if (xy_ultra.confidence_flag)
		{
			set_velocity(0, 0, LANDING_RATE, sp);
			set_yaw(0, sp);
			api.update_setpoint(sp);
			return 0;
		}
		else
		{
			controller_land.calc();
			uint8_t flag_v = 0, flag_h = 0;
			if ((fabs(height - navigator_land.endpoint.z) < 0.3))
			{
				controller_land.d3_ctrl.velocity.vz = 0;
				printf("z!\n");
				flag_v = 1;				
			}
			if ((xy_dis < 0.3))
			{
				controller_land.d3_ctrl.velocity.vx = 0;
				controller_land.d3_ctrl.velocity.vy = 0;
				printf("x, y!\n");
				flag_h = 1;
			}
			if (flag_v && flag_h)
			{
				xy_network.mission_command.cloud_control_running = 0;
			}
			
			printf("controller_land.output: x = %.4f, y = %.4f, z = %.4f\n", controller_land.d3_ctrl.velocity.vx, controller_land.d3_ctrl.velocity.vy, controller_land.d3_ctrl.velocity.vz);
			set_ref_yaw_rate(controller_land.d3_ctrl, sp);
			api.update_setpoint(sp);
			return 0;
		}		
	}
}


// This correction method is only applicable when aircraft heads to north
ref_position_vision_t correction_vision(ref_position_vision_t vision_raw, autopilot_interface_c api, float height)
{
	ref_position_vision_t vision_corrected;
	float pitch = api.current_messages.attitude.pitch;				//pitch > 0: head up
	float roll = api.current_messages.attitude.roll;				//roll > 0: right down
	if ( (fabs(pitch) > PI / 2) || (fabs(roll) > PI / 2))
	{
		vision_corrected.x = vision_raw.x;
		vision_corrected.y = vision_raw.y;
	}
	else
	{
		vision_corrected.x = vision_raw.x - (height * tan(roll) * roll / fabs(roll));
		vision_corrected.y = vision_raw.y - (height * tan(pitch) * pitch / fabs(pitch));
	}
	return vision_corrected; 
}


// ------------------------------------------------------------------------------
//   Quit Signal Handler
// ------------------------------------------------------------------------------
// this function is called when you press Ctrl-C
void
quit_handler( int sig )
{
	printf("\n");
	printf("TERMINATING AT USER REQUEST\n");
	printf("\n");

	// autopilot interface
	try {
		autopilot_interface_quit->handle_quit(sig);
	}
	catch (int error){}

	// serial port
	try {
		serial_port_quit->handle_quit(sig);
	}
	catch (int error){}
	
	//wireless serial
	try {
		wireless_serial_quit->handle_quit(sig);
	}
	catch (int error){}

	// end program here
	exit(0);

}


// ------------------------------------------------------------------------------
//   Distance calculation
// ------------------------------------------------------------------------------


//float dis_cal(mavlink_local_position_ned_t point_1, mavlink_local_position_ned_t point_2)
float dis_cal(mavlink_local_position_ned_t point_1, waypoint_t point_2)
{
	return sqrt(pow((point_1.x - point_2.x), 2) + pow((point_1.y - point_2.y), 2));
}




float error_cal(mavlink_set_position_target_local_ned_t point_1, mavlink_local_position_ned_t point_2)
{
	return sqrt(pow((point_1.x - point_2.x), 2) + pow((point_1.y - point_2.y), 2) + pow((point_1.z - point_2.z), 2));
}


// param 1, 2, 3 are mapped to horizontal controller, param 4, 5, 6 are mapped to vertical controller
void set_pid_online(controller_3D_posPID_c &controller_1, controller_angPID_c &controller_2, d3_PID_t &PID_t_1, PID_kit_t &PID_t_2, float &radius, xy_network_c &network_commander)
{
	printf("pid-flag = %d\n", network_commander.mission_command.pid.pid_flag);
	if (network_commander.mission_command.get_pid_test_status())
	{
		pid_test_t pid;
		pid_write_to_file(network_commander.mission_command.pid);
		pid_load(&pid);
		if ((pid.p1 < 1.0) && (pid.p1 >= 0))
		{
			PID_t_1.PID_x.Kp = pid.p1;
			PID_t_1.PID_y.Kp = pid.p1;
		}

		if ((pid.p2 < 1.0) && (pid.p2 >= 0))
		{
			PID_t_1.PID_x.Ki = pid.p2;
			PID_t_1.PID_y.Ki = pid.p2;
		}

		if ((pid.p3 < 1.0) && (pid.p3 >= 0))
		{
			PID_t_1.PID_x.Kd = pid.p3;
			PID_t_1.PID_y.Kd = pid.p3;
		}

		if ((pid.p4 < 3.0) && (pid.p4 >= 0))
		{
			PID_t_2.Kp = pid.p4;
		}

		if ((pid.p5 < 1.0) && (pid.p5 >= 0))
		{
			PID_t_2.Ki = pid.p5;
		}

		if ((pid.p6 < 1.0) && (pid.p6 >= 0))
		{
			PID_t_2.Kd = pid.p6;
		}

		controller_1.set(PID_t_1);
		controller_2.set(PID_t_2.Kp, PID_t_2.Ki, PID_t_2.Kd, UPPER_YAW, LOWER_YAW);
		radius = pid.p7;
		
		network_commander.mission_command.set_command_ack(COMMAND_ACK, XYI_CMD_PID_TEST, XYI_RESULT_ACCEPTED);
		network_commander.mission_command.reset_pid_test_status();
		//printf("PID_t.PID_x.Kp = %f\n", PID_t.PID_x.Kp);
	}
	//network_commander.mission_command.set_command_ack(COMMAND_ACK, XYI_CMD_PID_TEST, XYI_RESULT_ACCEPTED);
}


void distance_transformation(float x_in, float y_in, float &x_out, float &y_out, float height, xy_vision_c vision)
{
	Size sz(1280, 720);
	float u = sz.width * x_in;
	float v = sz.width * y_in;
	printf("x_in = %.4f, y_in = %.4f\n", x_in, y_in);
	//vision.point2loc(u, v, height * 100, "camconf1.xml", x_out, y_out);
	x_out = u * COE_IMAGE_TO_DIS * height;
	y_out = v * COE_IMAGE_TO_DIS * height;
}


// ------------------------------------------------------------------------------
//   Main
// ------------------------------------------------------------------------------
int
main(int argc, char **argv)
{
	// This program uses throw, wrap one big try/catch here	
	try
	{
		int result = top(argc,argv);
		return result;
	}

	catch ( int error )
	{
		fprintf(stderr,"xunyi_mav_control threw exception %i \n" , error);
		return error;
	}
}


